#include <ceres/ceres.h>
#include "../../util/mathfunc.h"
#include "../include/PBAFactors.h"

namespace Estimator
{
using namespace Eigen;

// Evaluate( main_achor, associate_anchor, parallax)
bool CeresPBAFactor3Nodes::Evaluate(double const *const *parameters, double *residuals,
                                    double **jacobians) const
{
    //map these variables from data pointer to Eigen variables..
    Eigen::Map<const Estimator::TransformType> MainPose(parameters[0], 3, 4);
    Eigen::Map<const Estimator::TransformType> AssoPose(parameters[1], 3, 4);
    Eigen::Map<const Estimator::FeatureType> feature(parameters[2], 5, 1);
    //std::cout << "============Evaluate 3Nodes ===========" << std::endl;

    Eigen::Map<Eigen::Vector3d> resVec(residuals);

    const Eigen::Matrix3d &Rm = MainPose.topLeftCorner<3, 3>();
    const Eigen::Vector3d &pm = MainPose.topRightCorner<3, 1>(); 
    const Eigen::Matrix3d &Ra = AssoPose.topLeftCorner<3, 3>();
    const Eigen::Vector3d &pa = AssoPose.topRightCorner<3, 1>();
    const Eigen::Matrix3d Rt = Ra.transpose();
    // Eigen::Matrix3d Rcb = Rbc.transpose();
    const Vector3d a = pm - pa; //+ (Rm - Ra) * tbc;                // the vector between associate frame and main frame.
    const Vector3d dirMainWorld = Rm * feature.segment<3>(2);       // direction in world coordinate...

    Eigen::Vector3d acrossn = a.cross(dirMainWorld); // cross product between camera vector and directional main anhcor
    double acrossnNorm = acrossn.norm();
    acrossn.normalize();
    Eigen::Vector3d aProj2n = a.dot(dirMainWorld) * dirMainWorld; // the projection vector of relative vector on main anchor.

    Eigen::Vector3d N = feature[0] * acrossnNorm * dirMainWorld + feature[1] * (a - aProj2n);
    Eigen::Vector3d fc = Rt * N;

    Eigen::Matrix3d dX = Estimator::dDirection(fc); // jacobian for the direction error in fc.
    
    if (jacobians != nullptr)
    {
        const Matrix3d N2 = -feature[0] * (dirMainWorld * acrossn.transpose() * skew(dirMainWorld)) - feature[1] * (dirMainWorld * dirMainWorld.transpose());

        if (jacobians[0] != nullptr)
        {
            Eigen::Matrix3d N1 = feature[0] * (acrossnNorm * Matrix3d::Identity() + dirMainWorld * acrossn.transpose() * skew(a)) - feature[1] * (dirMainWorld * a.transpose() + a.dot(dirMainWorld) * Matrix3d::Identity());

            Eigen::Matrix<double, 3, 6> dnm;
            //dnm << -skew(dirMainWorld), Matrix3d::Zero();
            dnm.topLeftCorner<3, 3>() = -skew(dirMainWorld);
            dnm.topRightCorner<3, 3>() = Matrix3d::Zero();
            Eigen::Matrix<double, 3, 6> dam;
            dam.topLeftCorner<3, 3>() = -skew(pm);//Matrix3d::Zero();
            dam.topRightCorner<3, 3>() = Matrix3d::Identity();

            Matrix36d tempJacobianMain = Rt * (N1 * dnm + N2 * dam + feature[1] * dam);

            Eigen::Map<Estimator::PoseJacobianType> JacobianMain(jacobians[0]);
            JacobianMain.setZero();
            // chain rule
            JacobianMain.topLeftCorner<3, 6>().noalias() = dX * tempJacobianMain;
        }
        if (jacobians[1] != nullptr)
        {
            Eigen::Map<Estimator::PoseJacobianType> JacobianAsso(jacobians[1]);
            JacobianAsso.setZero();

            Eigen::Matrix<double, 3, 6> daa;
            daa.topLeftCorner<3, 3>() = skew(pa);
            daa.topRightCorner<3, 3>() = -Matrix3d::Identity();

            Matrix36d tempJacobianCurr, tempJacobianAsso = Rt * N2 * daa;
            tempJacobianCurr.topLeftCorner<3, 3>().noalias() = Rt * skew(N + feature[1] * pa);
            tempJacobianCurr.topRightCorner<3, 3>().noalias() = -feature[1] * Rt;
            // tempJacobianCurr = Rcb * tempJacobianCurr;
            JacobianAsso.topLeftCorner<3, 6>().noalias() = dX * (tempJacobianAsso + tempJacobianCurr);
        }
        if (jacobians[2] != nullptr)
        {
            Eigen::Map<Estimator::FeatureJacobianType> JacobianFeature(jacobians[2]);
            JacobianFeature.setZero();

            Eigen::Vector3d Jf = Rt * (-feature[1] * acrossnNorm * dirMainWorld + feature[0] * (a - aProj2n));
            JacobianFeature.topLeftCorner<3, 1>().noalias() = dX * Jf;
        }
    }

    // fc.normalize();
    resVec = fc - mMeasurement_;

    return true;
}


bool CeresPBAFactor4Nodes::Evaluate(double const* const* parameters, double* residuals, 
                        double** jacobians)const
{
    // map these variables from data pointer to Eigen variables..
    Eigen::Map<const Estimator::TransformType> MainPose(parameters[0], 3, 4);
    Eigen::Map<const Estimator::TransformType> AssoPose(parameters[1], 3, 4);
    Eigen::Map<const Estimator::TransformType> CurrentPose(parameters[2], 3, 4);
    Eigen::Map<const Estimator::FeatureType> feature(parameters[3], 5, 1);

    Eigen::Map<Eigen::Vector3d> resVec(residuals);

    const Eigen::Matrix3d &Rm = MainPose.topLeftCorner<3, 3>();
    const Eigen::Vector3d &pm = MainPose.topRightCorner<3, 1>();
    // const Eigen::Matrix3d &Ra = AssoPose.topLeftCorner<3, 3>();
    const Eigen::Vector3d &pa = AssoPose.topRightCorner<3, 1>();
    const Eigen::Matrix3d &R = CurrentPose.topLeftCorner<3, 3>();
    const Eigen::Vector3d &p = CurrentPose.topRightCorner<3, 1>();

    const Eigen::Matrix3d Rt = R.transpose();

    const Vector3d a = pm - pa; //+ (Rm - Ra) * tbc;                 // the vector between associate frame and main frame.
    const Vector3d dirMainWorld = Rm*feature.segment<3>(2); //Rm * Rbc * feature.segment<3>(2); // direction in world coordinate...
    const Vector3d b = pm - p; //+ (Rm - R) * tbc;
    Eigen::Vector3d acrossn = a.cross(dirMainWorld); // cross product between camera vector and directional main anhcor
    double acrossnNorm = acrossn.norm();
    acrossn.normalize();
    Eigen::Vector3d aProj2n = a.dot(dirMainWorld) * dirMainWorld; // the projection vector of relative vector on main anchor.

    // Eigen::Matrix3d Rcwi = Rt;//Rcb * Rt;

    Eigen::Vector3d N = feature[0] * acrossnNorm * dirMainWorld + feature[1] * (b - aProj2n);
    Eigen::Vector3d fc = Rt * N;

    Eigen::Matrix3d dX = Estimator::dDirection(fc);

    if (jacobians != nullptr)
    {
        const Matrix3d N2 = -feature[0] * (dirMainWorld * acrossn.transpose() * skew(dirMainWorld)) - feature[1] * (dirMainWorld * dirMainWorld.transpose());

        if(jacobians[0] != nullptr)
        {
            const Matrix3d N1 = feature[0] * (acrossnNorm * Matrix3d::Identity() + dirMainWorld * acrossn.transpose() * skew(a)) - feature[1] * (dirMainWorld * a.transpose() + a.dot(dirMainWorld) * Matrix3d::Identity());
            Eigen::Matrix<double, 3, 6> dnm = Eigen::Matrix<double,3,6>::Zero();
            //dnm << -skew(dirMainWorld), Matrix3d::Zero();
            dnm.topLeftCorner<3, 3>() = -skew(dirMainWorld);
            dnm.topRightCorner<3, 3>() = Matrix3d::Zero();
            Eigen::Matrix<double, 3, 6> dam = Eigen::Matrix<double,3,6>::Zero();
            dam.topLeftCorner<3, 3>() = -skew(pm);//Rm * tbc + pm);
            dam.topRightCorner<3, 3>() = Matrix3d::Identity();

            Matrix36d Jm = Rt * (N1 * dnm + N2 * dam + feature[1] * dam);

            Eigen::Map<Estimator::PoseJacobianType> JacobianMain(jacobians[0]);
            JacobianMain.setZero();
            JacobianMain.topLeftCorner<3, 6>().noalias() = dX * Jm;
        }
        if(jacobians[1] != nullptr)
        {
            Eigen::Matrix<double, 3, 6> daa = Eigen::Matrix<double,3,6>::Zero();
            // daa << skew(Ra * tbc + pa), -Matrix3d::Identity();
            daa.topLeftCorner<3, 3>() = skew(pa);//Ra * tbc + pa);
            daa.topRightCorner<3, 3>() = -Matrix3d::Identity();
            // dNa =  N2* daa ;
            Eigen::Matrix<double, 3, 6> Ja = Rt * N2 * daa;

            Eigen::Map<Estimator::PoseJacobianType> JacobianAsso(jacobians[1]);
            JacobianAsso.setZero();
            JacobianAsso.topLeftCorner<3, 6>().noalias() = dX * Ja;
        }
        if(jacobians[2] != nullptr)
        {
            Eigen::Map<Estimator::PoseJacobianType> JacobianCurr(jacobians[2]);
            JacobianCurr.setZero();
            Eigen::Matrix<double, 3, 6> Ji =  Eigen::Matrix<double,3,6>::Zero();
            Ji.topLeftCorner<3, 3>().noalias() = Rt * skew(N + feature[1] * p);
            Ji.topRightCorner<3, 3>().noalias() = -feature[1] * Rt;
            //Ji << Rt * skew(N + feature[1] * p), -feature[1] * Rt;
            //Ji = Rcb * Ji;

            JacobianCurr.topLeftCorner<3, 6>() = dX * Ji;
        }
        if(jacobians[3] != nullptr)
        {
            Eigen::Map<Estimator::FeatureJacobianType> JacobianFeature(jacobians[3]);
            JacobianFeature.setZero();
            // JacobianFeature.topLeftCorner(3, 1) = dX * temJacobianF;
            Vector3d Jf = Rt * (-feature[1] * acrossnNorm * dirMainWorld + feature[0] * (b - aProj2n));
            JacobianFeature.topLeftCorner<3, 1>().noalias() = dX * Jf;
        }
    }

    // fc.normalize();
    // fc.normalized();
    resVec = fc - mMeasurement_;

    return true;
}


bool CeresTransOnlyFactor3Nodes::Evaluate(double const* const* parameters, double* residuals, 
                        double** jacobians)const
{

   // map these variables from data pointer to Eigen variables..
    Eigen::Map<const Vector3d> pm(parameters[0]);
    Eigen::Map<const Vector3d> pa(parameters[1]);
    Eigen::Map<Eigen::Vector3d> resVec(residuals);

    Eigen::Vector3d Nij = mBzj * (pa - pm) + msintheta * (pm - pa);
    Eigen::Vector3d Rvij = mRoti * mMeasurement_;
    Eigen::Matrix3d Roti = skew(Rvij);

    resVec =  Roti* Nij;

    // std::cout << "3nodes: " << resVec[0] << ", " << resVec[1] << ", " <<  resVec[2] << std::endl;
    if (jacobians != nullptr)
    {
        if (jacobians[0] != nullptr)
        {
            Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>> JacobianMain(jacobians[0]);
            JacobianMain.setZero();
            JacobianMain = Roti*(msintheta * Eigen::Matrix3d::Identity() - mBzj);
        }
        if (jacobians[1] != nullptr)
        {
            Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>> JacobianAsso(jacobians[1]);
            JacobianAsso.setZero();
            JacobianAsso = Roti*(mBzj - msintheta * Eigen::Matrix3d::Identity());
        }
    }

    return true;
}

bool CeresTransOnlyFactor4Nodes::Evaluate(double const* const* parameters, double* residuals, 
                        double** jacobians)const
{
    // map these variables from data pointer to Eigen variables..
    Eigen::Map<const Vector3d> pm(parameters[0]);
    Eigen::Map<const Vector3d> pa(parameters[1]);
    Eigen::Map<const Vector3d> pi(parameters[2]);
    Eigen::Map<Eigen::Vector3d> resVec(residuals);

    Eigen::Vector3d Nij = mBzj * (pa - pm) + msintheta * (pm - pi);
    Eigen::Vector3d Rvij = mRoti * mMeasurement_;
    Eigen::Matrix3d Roti = skew(Rvij);
    
    resVec =  Roti* Nij;

    // std::cout << "4nodes: " << resVec[0] << ", " << resVec[1] << ", " << resVec[2] << std::endl << mBzj << std::endl << mRoti << std::endl;

    if (jacobians != nullptr)
    {
        if (jacobians[0] != nullptr)
        {
            Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> JacobianMain(jacobians[0]);
            JacobianMain.setZero();
            JacobianMain = Roti*(msintheta*Eigen::Matrix3d::Identity() - mBzj);
        }
        if (jacobians[1] != nullptr)
        {
            Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> JacobianAsso(jacobians[1]);
            JacobianAsso.setZero();
            JacobianAsso = Roti* mBzj;
        }
        if(jacobians[2] != nullptr)
        {
            Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>> JacobianCurr(jacobians[2]);
            JacobianCurr.setZero();
            JacobianCurr = Roti*(-msintheta*Eigen::Matrix3d::Identity());
        }
    }

    return true;

}

}